#include "awesome-o.h"
#include "motor.h"
#include "leds.h"
#include "switch.h"

#define BACK_RIGHT  (0)
#define FRONT_RIGHT (1)
#define FRONT_LEFT  (2)
#define BACK_LEFT   (3)



// this gets updated in the interrupt and read all over the code
// it must be volatile to prevent reading stale versions
volatile time_type time = 0;
time_type get_current_time()
{
	return time;
}

#define SWITCH_STATE_OPEN   (1)
#define SWITCH_STATE_CLOSED (2)

struct switch_state_t {
	int 		state;
	time_type 	start_of_change;
	int 		change_detected;
};

volatile struct switch_state_t switch_state[4];

static void init_switch_state(int leg)
{
	if (is_switch_open(leg))
		switch_state[leg].state = SWITCH_STATE_OPEN;
	else 
		switch_state[leg].state = SWITCH_STATE_CLOSED;
	switch_state[leg].change_detected = 0;
}

static void init_switch_states()
{
	for (int i = 0; i < 4; i++)
		init_switch_state(i);
}



#define DELAY_SKIP_SPIKES 3
static void update_switch_state(int leg)
{
	if ((is_switch_open(leg)   && switch_state[leg].state == SWITCH_STATE_CLOSED) ||
		(is_switch_closed(leg) && switch_state[leg].state == SWITCH_STATE_OPEN)) {
		// exista o modificare a starii switchului fata de ce stiam ca e pana acum

		if (!switch_state[leg].change_detected) {
			// nu s-a detectat pana acum o schimbare, asta e primul moment in care detectam schimbarea
			switch_state[leg].change_detected = 1;
			switch_state[leg].start_of_change = get_current_time();
		} else {
			// state change was detected some time ago
			// if enough time has passed, update the state
			if (get_current_time() >= switch_state[leg].start_of_change + DELAY_SKIP_SPIKES) {
				if (is_switch_open(leg))
					switch_state[leg].state = SWITCH_STATE_OPEN;
				else
					switch_state[leg].state = SWITCH_STATE_CLOSED;
				switch_state[leg].change_detected = 0;
			}
		}
	}
}

static int get_switch_state(int leg)
{
	return switch_state[leg].state;
}


SIGNAL(SIG_OUTPUT_COMPARE1A)
{
	time ++;

	for(int i = 0; i < 4; i++)
		update_switch_state(i);

/*
	for(int i = 0; i < 4; i++)
		if 	(is_switch_closed(i))
	   		led_on(i);	
		else 
			led_off(i);
*/

}





// Initializare timer 1 compare match A pentru a avea un timer la nivel de 1 secunda, 16MHz
static void init_timer()
{
	// 16000000 Hz / ((TIME_LIMIT=15625) * 1024 (prescaller)) = 1 Hz
	// -> intrerupere cu frecventa de 1 Hz -> o intrerupere pe secunda.
	//const int TIME_LIMIT = 15625;

	const int TIME_LIMIT = 156;
	OCR1A   = TIME_LIMIT;
	TCCR1A  = 0;
	TCCR1B  = (1<<WGM12)|(1<<CS12)|(1<<CS10);
	TIMSK  |= _BV(OCIE1A);
}


/*****************************************************/
/*                STATE                              */
/*****************************************************/




static time_type duration_closed = 0;
static time_type duration_open   = 0;
static time_type duration_total  = 0;



static void skip_state(int leg, int state)
{
	while(get_switch_state(leg) == state)
		;
}

static void skip_open_then_closed(int leg)
{
	// skip a open state
	skip_state(leg, SWITCH_STATE_OPEN);
	// skip a closed state
	skip_state(leg, SWITCH_STATE_CLOSED);
}

static void skip_closed_then_open(int leg)
{
	// skip a closed state
	skip_state(leg, SWITCH_STATE_CLOSED);
	// skip a open state
	skip_state(leg, SWITCH_STATE_OPEN);
}

static void duration_init()
{
	int leg = 0;
	time_type start_open, stop_open, start_closed, stop_closed;

	move(leg, ROT_TRIGONOMETRIC);

	skip_open_then_closed(leg);


	// count time in open state
	start_open = get_current_time();
	// skip a open state
	skip_state(leg, SWITCH_STATE_OPEN);
	stop_open = get_current_time();


	// count time in closed state
	start_closed = get_current_time();
	// skip a closed state
	skip_state(leg, SWITCH_STATE_CLOSED);
	stop_closed = get_current_time();


	stop_motor(leg);

	duration_open   = stop_open       - start_open;
	duration_closed = stop_closed     - start_closed;
	duration_total  = duration_closed + duration_open;
}



static int is_absolute_front_leg(int leg)
{
	return (leg == FRONT_LEFT) || (leg == FRONT_RIGHT);
}


static void init_leg_positions_canonical()
{
	int i;
	int leg[] = {BACK_LEFT, BACK_RIGHT, FRONT_LEFT, FRONT_RIGHT};
	int dir[] = {ROT_TRIGONOMETRIC, ROT_CLOCKWISE, ROT_TRIGONOMETRIC, ROT_CLOCKWISE};
	for (i = 0; i < 4; i++)
	{
		move(leg[i], dir[leg[i]]);
		if (is_absolute_front_leg(leg[i]))
			skip_closed_then_open(leg[i]);
		else
			skip_open_then_closed(leg[i]);
		stop_motor(leg[i]);
	}
}


static void wait(int wait_cycles)
{
	time_type entry_time = get_current_time();
	time_type deadline = entry_time + wait_cycles;
	while(get_current_time() <= deadline)
		; // busy waiting like there's no tomorrow
}


static void serial_rotation(int leg, int dir, time_type duration)
{
	move(leg, dir);
	wait(duration);
	stop_motor(leg);
	wait(1);
}



static void init_leg_positions_pole_positions(int back_leg, int front_leg)
{
	serial_rotation(back_leg,  ROT_CLOCKWISE,     duration_total/6);
	serial_rotation(front_leg, ROT_TRIGONOMETRIC, duration_total/6);
}





enum dir_type {
	DIR_FORWARD,
	DIR_LEFT,
	DIR_BACKWARD,
	DIR_RIGHT,
};

static int is_stop_condition(enum dir_type dir, int side_a, int side_b, int odd_step, int back_leg[2])
{
	switch(dir) {
	case DIR_FORWARD:
		return get_switch_state(back_leg[side_b]) == SWITCH_STATE_CLOSED;
		break;
	case DIR_LEFT:
		if (!odd_step)
			return get_switch_state(back_leg[side_a]) == SWITCH_STATE_OPEN;
		else
			return get_switch_state(back_leg[side_a]) == SWITCH_STATE_CLOSED;
		break;
	case DIR_BACKWARD:
		return get_switch_state(back_leg[side_b]) == SWITCH_STATE_OPEN;
		break;
	case DIR_RIGHT:
		if (!odd_step)
			return get_switch_state(back_leg[side_a]) == SWITCH_STATE_CLOSED;
		else
			return get_switch_state(back_leg[side_b]) == SWITCH_STATE_OPEN;
		break;
	}
	return 0;
}

static void run_in_strait_direction(int back_leg[], int front_leg[], enum dir_type dir, int nr_steps)
{
	time_type reltime = 0, old_reltime = 0;
	time_type abstime;
	int side_a = 1;
	int side_b = 0;
	int dir_back_leg = ROT_CLOCKWISE;
	init_leg_positions_canonical();
	init_leg_positions_pole_positions(back_leg[side_b], front_leg[side_b]);
	
	abstime = get_current_time();
	for(int i = 0; i < nr_steps; /* i gets incremented when a whole step is made*/) {
		do {
			old_reltime = reltime;
			reltime = get_current_time() - abstime;
		} while(old_reltime == reltime);

		if (reltime == 1 * duration_total / 15) {
			// set the start rules: front legs move in oposite dirs and back legs are stopped
			move(front_leg[side_a], dir_back_leg);
			move(front_leg[side_b], ROT_STOPPED);
			move(back_leg[side_a],  ROT_STOPPED);
			move(back_leg[side_b],  ROT_STOPPED);
		} else if (reltime == 2 * duration_total / 15) {
			move(front_leg[side_b], dir_back_leg);
		} else if (reltime == 2 * duration_total / 10) {
			// now start the back legs, but don't check the switch states.
			move(back_leg[side_a], - dir_back_leg);
			move(back_leg[side_b], - dir_back_leg);
		} else if (reltime >= 3 * duration_total / 10) {
			// maintain same motion and check the switch state
			// when switches change, swap feet
			if (is_stop_condition(dir, side_a, side_b, i % 2, back_leg)) {
				stop_motors();
				abstime = get_current_time();
				{int aux; aux = side_a; side_a = side_b; side_b = aux;}
				dir_back_leg = - dir_back_leg; //change dir.
				i++; // a new step was done!
			} else {
				// do nothing. just wait for the legs to move.
			}
		}
	}
}

static void rotate_in_strait_direction(int back_leg[], int front_leg[], int rotation_direction, int nr_steps)
{
	time_type reltime = 0, old_reltime = 0;
	time_type abstime;
	int side_a = 1;
	int side_b = 0;
	init_leg_positions_canonical();
	init_leg_positions_pole_positions(back_leg[side_b], front_leg[side_b]);
	
	abstime = get_current_time();
	int state = 0;
	for(int i = 0; i < nr_steps; /* i gets incremented when a whole step is made*/) {
		do {
			old_reltime = reltime;
			reltime = get_current_time() - abstime;
		} while(old_reltime == reltime);
		all_leds_off();
		led_on(state);
		if ((state == 0) || (state == 1)) {
			if (reltime == 1 * duration_total / 15) {
				move(back_leg [side_a], rotation_direction);
				move(front_leg[side_b], rotation_direction);
				move(back_leg [side_b], ROT_STOPPED);
				move(front_leg[side_a], ROT_STOPPED);
			} else if (reltime == duration_total / 3) {
				stop_motors();
			} else if (reltime >= duration_total / 3 + 1 * duration_total / 15) {
				abstime = get_current_time();
				{int aux; aux = side_a; side_a = side_b; side_b = aux;}
				state ++;
				rotation_direction = - rotation_direction;
			}
		} else {
			if (reltime == 1 * duration_total / 15) {
				move(back_leg [side_a], - rotation_direction);
				move(front_leg[side_b], - rotation_direction);
				move(back_leg [side_b], ROT_STOPPED);
				move(front_leg[side_a], ROT_STOPPED);
			} else if (reltime >= duration_total / 4) {
				// maintain same motion and check the switch state
				// when switches change, swap feet
				if (get_switch_state(back_leg[(state==2)?side_a:side_b]) != SWITCH_STATE_CLOSED) {
					stop_motors();
					abstime = get_current_time();
					{int aux; aux = side_a; side_a = side_b; side_b = aux;}
					state ++;
					rotation_direction = - rotation_direction;
					if (state == 4) {
						i++; // a new step was done!
						state = 0;
					}
				} else {
					// do nothing. just wait for the legs to move.
				}
			}
		}
	}
}

static void samba(int back_leg[], int front_leg[], int nr_steps)
{
	time_type reltime = 0, old_reltime = 0;
	time_type abstime;
	int side_a = 1;
	int side_b = 0;
	int dir_back_leg = ROT_CLOCKWISE;
	init_leg_positions_canonical();
	init_leg_positions_pole_positions(back_leg[side_b], front_leg[side_b]);
	
	abstime = get_current_time();
	for(int i = 0; i < nr_steps; /* i gets incremented when a whole step is made*/) {
		do {
			old_reltime = reltime;
			reltime = get_current_time() - abstime;
		} while(old_reltime == reltime);


		if (reltime == 1 * duration_total / 15) {
			move(back_leg [side_a], dir_back_leg);
			move(front_leg[side_b], dir_back_leg);
			move(back_leg [side_b], ROT_STOPPED);
			move(front_leg[side_a], ROT_STOPPED);
		} else if (reltime == duration_total / 4) {
			stop_motors();
		} else if (reltime == duration_total / 4 + 1 * duration_total / 15) {
			move(back_leg [side_a], ROT_STOPPED);
			move(front_leg[side_b], ROT_STOPPED);
			move(back_leg [side_b], dir_back_leg);
			move(front_leg[side_a], dir_back_leg);
		} else if (reltime >= duration_total / 4 + 3 * duration_total / 15) {
			// maintain same motion and check the switch state
			// when switches change, swap feet
			if (get_switch_state(back_leg[side_b]) != SWITCH_STATE_CLOSED) {
				stop_motors();
				abstime = get_current_time();
				{int aux; aux = side_a; side_a = side_b; side_b = aux;}
				dir_back_leg = - dir_back_leg; //change dir.
				i++; // a new step was done!
			} else {
				// do nothing. just wait for the legs to move.
			}
		}
	}
}




static void move_to_left(int nr_steps)
{
	int back_leg[]  = {BACK_RIGHT, FRONT_RIGHT};
	int front_leg[] = {BACK_LEFT,  FRONT_LEFT };
	run_in_strait_direction(back_leg, front_leg, DIR_LEFT, nr_steps);
}

static void move_to_right(int nr_steps)
{
	int back_leg[]  = {FRONT_LEFT,  BACK_LEFT };
	int front_leg[] = {FRONT_RIGHT, BACK_RIGHT};
	run_in_strait_direction(back_leg, front_leg, DIR_RIGHT, nr_steps);
}

static void move_to_front(int nr_steps)
{
	int back_leg[]  = {BACK_LEFT,  BACK_RIGHT};
	int front_leg[] = {FRONT_LEFT, FRONT_RIGHT};
	run_in_strait_direction(back_leg, front_leg, DIR_FORWARD, nr_steps);
}

static void move_to_back(int nr_steps)
{
	int back_leg[]  = {FRONT_RIGHT, FRONT_LEFT};
	int front_leg[] = {BACK_RIGHT,  BACK_LEFT };
	run_in_strait_direction(back_leg, front_leg, DIR_BACKWARD, nr_steps);
}

static void rotate_to_left(int nr_steps)
{
	int back_leg[]  = {BACK_LEFT,  BACK_RIGHT};
	int front_leg[] = {FRONT_LEFT, FRONT_RIGHT};
	rotate_in_strait_direction(back_leg, front_leg, ROT_TRIGONOMETRIC, nr_steps);
}

static void rotate_to_right(int nr_steps)
{
	int back_leg[]  = {BACK_LEFT,  BACK_RIGHT};
	int front_leg[] = {FRONT_LEFT, FRONT_RIGHT};
	rotate_in_strait_direction(back_leg, front_leg, ROT_CLOCKWISE, nr_steps);
}


int main(void)
{
	PORTD = _BV(m2_In1)|_BV(Sw1)|_BV(Sw2);
    DDRD = _BV(m2_In2)|_BV(m2_In1)|_BV(led1)|_BV(led2);
	_NOP();


	PORTA = _BV(m1_In1)|_BV(Sw0)|_BV(m0_In1)|_BV(Sw3);	
	DDRA = _BV(m1_In1)|_BV(m1_In2)|_BV(m0_In1)|_BV(m0_In2)|_BV(led3)|_BV(led0);
	_NOP();

	PORTC = _BV(m3_In1);
	DDRC = _BV(m3_In1)|_BV(m3_In2);
	_NOP();

	stop_motors();

	init_switch_states();

	init_timer();
	sei();

		
	duration_init();
	for(;;) {
		rotate_to_left(5);
		rotate_to_right(5);
	}
	int nr_steps = 5;
	for(;;) {
		wait(duration_total);
		move_to_right(nr_steps);
		wait(duration_total);
		move_to_left(nr_steps);
		wait(duration_total);
		move_to_back(nr_steps);
		wait(duration_total);
		move_to_front(nr_steps);
	}
}
